/*
 #include <motor_task.h>
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-07     Hao       the first version
 */
#include "motor_task.h"
//#include <tool/vofa.h>

#include "encoder.h"
#include "driver.h"
#include "inline_current_sense.h"
#include "bldc_motor.h"

Encoder encoder = Encoder(AS5600);
Driver driver = Driver();
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50, 0, 1, 2);   // 采样电阻、放大倍数、ABC相对应的ADC通道
BLDCMotor motor = BLDCMotor(7); // 极对数

// 运行目标值，其实可以优化掉，直接设置motor属性，但是为了保留simplefoc中motor完整性，先不改了。
float motor_target;
// 单独定义一遍，主要是为了方便和其他线程中交互，因为CAN线程用C写的，和C中不能直接传递类对象
uint8_t motor_mode; // 当前运行模式，can_upper_task中修改电机运行模式使用
float motor_angle;  // 当前电机角度，can_upper_task中实时上报当前电机角度

static void motor_setup()
{
    encoder.init();
    driver.init();
    current_sense.init();

    motor.linkEncoder(&encoder);
    motor.linkDriver(&driver);
    motor.linkCurrentSense(&current_sense);

    motor_target = 0;                         // 控制目标值
    motor.torque_controller_ = FOC_CURRENT;     // 电流控制
    motor.motion_controller_ = ANGLE;        // 速度控制

    motor.init();

    // 如果知道就填上，第一次运行过后可以看看串口发送的消息，然后填上
    motor.zero_electric_angle_ = NOT_SET;       // 传感器0度时电机的电角度，不知道就填NOT_SET
    motor.encoder_direction_ = CW;              // 传感器方向 CCW CW，不知道就填UNKNOWN

    motor.initFOC();

    // 这个变量主要用于和can_upper_task、uart_debug_task之间的信息传递
    motor_mode = motor.motion_controller_;
}

#include <tool/vofa.h>
static void motor_loop()
{
    // can_upper_task、uart_debug_task会修改motor_mode、motor_target的值
    motor.motion_controller_ = (MotionControlType) motor_mode;

    motor.move(motor_target);
    motor.loopFOC();

    motor_angle = motor.shaft_angle_;       // can_upper_task会上传motor_angle的值

#if 0       // 此功能和SHELL共用同一个串口
    float dabug_data[2];
    dabug_data[0] = motor_target;
//    dabug_data[1] = motor.current_.q;
//    dabug_data[2] = motor.current_.d;
//    dabug_data[1] = motor.shaft_velocity_;
    dabug_data[1] = motor.shaft_angle_;
    vofa_justfloat_send(dabug_data, sizeof(dabug_data)/sizeof(float));
#endif
}

void motor_task(void *para)
{
    motor_setup();

    while (1)
    {
        motor_loop();
    }
}

// 串口调参
#if 0

#include <stdlib.h>

void motor_tar(int argc, char**argv)
{
    if (argc < 2)
    {
        return;
    }
    motor_target = atof(argv[1]);
}
MSH_CMD_EXPORT(motor_tar, motor_tar);

// 分别为pid参数
// motor_pid 11 22 33
void motor_pid(int argc, char**argv)
{
    if (argc < 4)
    {
        return;
    }
    float p = atof(argv[1]);
    float i = atof(argv[2]);
    float d = atof(argv[3]);
#if 0
    motor.PID_current_q_.p_ = p;
    motor.PID_current_q_.i_ = i;
    motor.PID_current_q_.d_ = d;
    motor.PID_current_d_.p_ = p;
    motor.PID_current_d_.i_ = i;
    motor.PID_current_d_.d_ = d;
#endif
#if 0
    motor.PID_velocity_.p_ = p;
    motor.PID_velocity_.i_ = i;
    motor.PID_velocity_.d_ = d;
#endif
#if 1
    motor.P_angle_.p_ = p;
    motor.P_angle_.i_ = i;
    motor.P_angle_.d_ = d;
#endif
}
MSH_CMD_EXPORT(motor_pid, motor_pid);

#endif


